import copy
import trimesh
import numpy as np
from numpy import linalg as LA
from trimesh.transformations import rotation_matrix, euler_matrix, transform_points, translation_matrix
from scipy.spatial.transform import Rotation
import random
import math
import sys
import os
from .model_exchange import Transform

import detail_design.models.detail_design.jxq.model_parameter_updatev1 as model_update
import detail_design.models.detail_design.jxq.group as group

import detail_design.models.detail_design.jxq.model_operator as model_operator

import detail_design.models.detail_design.jxq.model_helper as model_helper


def get_vertex_from_slice_plane(rpslist, origin, normal, axis_normal):
    vertexlist = []
    for rps in rpslist:
        vertexlist.append(model_helper.point_point2plane(rps, origin, normal))

    axisorigin = model_helper.point_point2plane([0, 0, 1], origin, normal)
    vertexdict = {}
    result = []
    for i in range(len(vertexlist)):
        v = vertexlist[i]
        value = ((v[0] - axisorigin[0]) / axis_normal[0])
        vertexdict[i] = value
    for index, v in sorted(vertexdict.items(), key=lambda kv: (kv[1], kv[0])):
        result.append(vertexlist[index])
    # cap.show()
    return result


def get_rps_all(rps_obj):
    rps_all = []
    for rps in rps_obj:
        rps_all += np.array([rps_obj[rps]['Point']]).tolist()
    return rps_all


def get_clamp_setting_down(workmesh, rps_obj, clamp, center=None, angle=0, first_clamp_point=None, outerst=None,distvalue=None):
    angle = angle / 180 * math.pi
    rps_all = rps_obj
    if center is None:
        center = np.mean(rps_all, axis=0)
    rps_center = np.mean(rps_all, axis=0)
    max_value = np.max(rps_all, axis=0)
    z_value = 85
    safe_constant = 25
    move_constant = 80
    if 'safe_z' in clamp.obj:
        z_value = clamp.obj['safe_z']
    if 'move_constant' in clamp.obj:
        move_constant = clamp.obj['move_constant']
    if 'safe_constant' in clamp.obj:
        safe_constant = clamp.obj['safe_constant']

    z = max_value[2] + z_value
    slice_plane_normal = [math.cos(angle + math.pi / 2), math.sin(angle + math.pi / 2), 0]

    axis_normal = np.array([math.cos(angle), math.sin(angle), 0])

    if first_clamp_point is None:

        possiblevertex = get_vertex_from_slice_plane(rps_all, center, slice_plane_normal, axis_normal)

        first_clamp_point_inner = possiblevertex[-1]
        first_clamp_point_inner[2] = z

        inner = model_operator.model_rotate(clamp, angle, [0, 0, 1], first_clamp_point_inner)
        inner = model_operator.move_clamp_pointTopoint(inner, inner.get_point('setting_pt'), first_clamp_point_inner)

        first_clamp_point_outer = possiblevertex[0]
        first_clamp_point_outer[2] = z

        outer = model_operator.model_rotate(clamp, angle + math.pi, [0, 0, 1], first_clamp_point_outer)
        outer = model_operator.move_clamp_pointTopoint(outer, outer.get_point('setting_pt'), first_clamp_point_outer)

        test_inner = model_operator.move_along_axis(inner, axis_normal, move_constant)
        test_outer = model_operator.move_along_axis(outer, -axis_normal, move_constant)

        inner_test = model_operator.detect_collision(test_inner, workmesh)
        outer_test = model_operator.detect_collision(test_outer, workmesh)
        if not inner_test and not outer_test:
            inner_point_box = trimesh.creation.box([10, 10, 10],
                                                   transform=translation_matrix(test_inner.get_point('setting_pt')))
            innerdist = model_operator.detect_distance_between_mesh_and_setting_box(inner_point_box, workmesh)

            outer_point_box = trimesh.creation.box([10, 10, 10],
                                                   transform=translation_matrix(test_outer.get_point('setting_pt')))
            outerdist = model_operator.detect_distance_between_mesh_and_setting_box(outer_point_box, workmesh)

            inner = model_operator.move_along_axis(inner, axis_normal, move_constant + safe_constant - int(innerdist))
            outer = model_operator.move_along_axis(outer, -axis_normal, move_constant + safe_constant - int(outerdist))

            outervertex = outer.get_point("setting_pt")
            outerdistvalue = LA.norm(outervertex[0:-1] - rps_center[0:-1])

            innervertex = inner.get_point("setting_pt")
            innerdistvalue = LA.norm(innervertex[0:-1] - rps_center[0:-1])

            if outerdistvalue <= innerdistvalue:
                clamp = outer
                clamp.obj['first_clamp_point'] = first_clamp_point_outer
                clamp.obj['outerst'] = True
                clamp.obj['distvalue'] = outerdist
            else:
                clamp = inner
                clamp.obj['first_clamp_point'] = first_clamp_point_inner
                clamp.obj['outerst'] = False
                clamp.obj['distvalue'] = innerdist
        elif inner_test and not outer_test:
            outer_point_box = trimesh.creation.box([10, 10, 10],
                                                   transform=translation_matrix(test_outer.get_point('setting_pt')))
            outerdist = model_operator.detect_distance_between_mesh_and_setting_box(outer_point_box, workmesh)
            # outerdist=model_operator.detect_distance_between_mesh(test_outer,workmesh)
            outer = model_operator.move_along_axis(outer, -axis_normal, move_constant + safe_constant - int(outerdist))
            clamp = outer
            clamp.obj['first_clamp_point'] = first_clamp_point_outer
            clamp.obj['outerst'] = True
            clamp.obj['distvalue'] = outerdist
        elif outer_test and not inner_test:
            inner_point_box = trimesh.creation.box([10, 10, 10],
                                                   transform=translation_matrix(test_inner.get_point('setting_pt')))
            innerdist = model_operator.detect_distance_between_mesh_and_setting_box(inner_point_box, workmesh)
            # innerdist=model_operator.detect_distance_between_mesh(test_inner,workmesh)
            inner = model_operator.move_along_axis(inner, axis_normal, move_constant + safe_constant - int(innerdist))
            clamp = inner
            clamp.obj['first_clamp_point'] = first_clamp_point_inner
            clamp.obj['outerst'] = False
            clamp.obj['distvalue'] = innerdist
        else:
            return None
    else:
        first_clamp_point[2] = z
        if outerst:
            outerdist = distvalue
            outer = model_operator.model_rotate(clamp, angle + math.pi, [0, 0, 1], first_clamp_point)
            outer = model_operator.move_clamp_pointTopoint(outer, outer.get_point('setting_pt'), first_clamp_point)
            outer = model_operator.move_along_axis(outer, -axis_normal, move_constant + safe_constant - int(outerdist))
            clamp = outer
            clamp.obj['first_clamp_point'] = first_clamp_point
            clamp.obj['outerst'] = True
            clamp.obj['distvalue'] = outerdist
        else:
            innerdist = distvalue
            inner = model_operator.model_rotate(clamp, angle, [0, 0, 1], first_clamp_point)
            inner = model_operator.move_clamp_pointTopoint(inner, inner.get_point('setting_pt'), first_clamp_point)
            inner = model_operator.move_along_axis(inner, axis_normal, move_constant + safe_constant - int(innerdist))
            clamp = inner
            clamp.obj['first_clamp_point'] = first_clamp_point
            clamp.obj['outerst'] = False
            clamp.obj['distvalue'] = innerdist
    if not isinstance(clamp.obj['first_clamp_point'],list):
        clamp.obj['first_clamp_point'] = clamp.obj['first_clamp_point'].tolist()
    clamp.obj['distvalue'] = int(clamp.obj['distvalue'])
    return clamp


def get_model_extend_to_z(clamp, base_z):
    if base_z is not None:
        for ele in clamp.obj['args']:
            if '支柱' in ele['name'] and 'H' in ele['name']:
                element_name = ele['name'][0:ele['name'].find('__')]
                bottom_vertex = clamp.get_point(element_name + "/bottom")
                H1 = ele['v']
                ele['v'] = H1 + bottom_vertex[2] - base_z
        new_obj = group.Body(clamp.obj)
        # from OCC.Display.SimpleGui import init_display
        # display, start_display, add_menu, add_function_to_menu = init_display('wx')
        # display.DisplayShape(new_obj.shape(),update=True)
        # start_display()
        model_operator.update_model_by_history(new_obj, clamp)
        new_clamp = new_obj
        return new_clamp
    return clamp


def get_model_update(workmesh, rps_obj, clamp, center=None, angle=0, move_constant=80, first_clamp_point=None,
                     base_z=None, outerst=None, distvalue=None,step=500,unit_name=None):
    clamp = get_clamp_setting_down(workmesh, rps_obj, clamp, center, angle, first_clamp_point, outerst,distvalue)
    rps_all = rps_obj


    new_clamp = model_update.model_update(obj=clamp, rps_coor=rps_all, env=None, section=None, air_point=None, lr=1,
                                          dx=1,
                                          iters=step)



    new_clamp = get_model_extend_to_z(new_clamp, base_z)


    clamp_shape = new_clamp
    return clamp_shape





if __name__ == "__main__":
    # import get_clamp_point_helper
    import os

    _BASEDIR = os.path.dirname(os.path.abspath(__file__))
    # obj=get_clamp_point_helper.parse_info(_BASEDIR+"/test_data/")
    # transform = get_clamp_point_helper.get_transform(obj['Extra']['BaseDirection'])
    workmesh = trimesh.load(_BASEDIR + "/test/test_data/work_mesh.stl")
    # workmesh.apply_transform(transform)

    rps_dic = {'RPS-1-AB-01': {'Point': np.array([1109.224, 126.86600000000004, -733.1]), 'Type': '夹紧'}}
    center = np.array([1109.224, 126.86600000000004, -733.1])
    center = np.array([1214.108, -60.197, -727.7])
    angle = 45
    rps_all = get_rps_all(rps_dic)
    print(rps_all)
    clamp_point_box = trimesh.creation.box([10, 10, 10],
                                           transform=translation_matrix(rps_all[0]))
    clamp = group.Body('Clamp01', local=True)
    clamp.show()
    clamp = get_model_update(workmesh, rps_dic, clamp, center, angle - 90, base_z=-1500)

    showResult(clamp, workmesh, clamp_point_box)

    # clamp.show(env=(workmesh+clamp_point_box))
